%正解：牛顿迭代法
function Out  = Stewart_IK_AlgebraFunc(Length)
x0=[0;0;20;0;0;0];
Out=x0;
x1=[0;0;20;0;0;0];
x1=x0-inv(myJacobi(x0,Length))*myfun(x0,Length);
while norm(x1-x0)>1e-3
    x0=x1;
    x1=x0-inv(myJacobi(x0,Length))*myfun(x0,Length);
end
Out = x1;
end
 
function f=myfun(x,Length)
%零初始位置时静平台位置
B1=[12;-14;0];
B2=[12;14;0];
B3=[6.12435565;17.39230485;0];
B4=[-18.12435565;3.39230485;0];
B5=[-18.12435565;-3.39230485;0];
B6=[6.12435565;-17.39230485;0];
%零初始位置时动平台位置
P1=[12;-2;0];
P2=[12;2;0];
P3=[-4.26794919;11.39230485;0];
P4=[-7.73205081;9.39230485;0];
P5=[-7.73205081;-9.39230485;0];
P6=[-4.26794919;-11.39230485;0];

x1=x(1);
x2=x(2);
x3=x(3);
a=x(4);
b=x(5);
g=x(6);
% l1=19.78;
% l2=24.81;
% l3=24.54;
% l4=21.25;
% l5=21.34;
% l6=18.2;
l1=Length(1);
l2=Length(2);
l3=Length(3);
l4=Length(4);
l5=Length(5);
l6=Length(6);
X=[x1;x2;x3];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
f1=(norm(X+R*P1-B1))^2-l1^2;
f2=(norm(X+R*P2-B2))^2-l2^2;
f3=(norm(X+R*P3-B3))^2-l3^2;
f4=(norm(X+R*P4-B4))^2-l4^2;
f5=(norm(X+R*P5-B5))^2-l5^2;
f6=(norm(X+R*P6-B6))^2-l6^2;
f=[f1;f2;f3;f4;f5;f6];
end
 
function J=myJacobi(x,Length)
%零初始位置时静平台位置
%零初始位置时静平台位置
B1=[12;-14;0];
B2=[12;14;0];
B3=[6.12435565;17.39230485;0];
B4=[-18.12435565;3.39230485;0];
B5=[-18.12435565;-3.39230485;0];
B6=[6.12435565;-17.39230485;0];
%零初始位置时动平台位置
P1=[12;-2;0];
P2=[12;2;0];
P3=[-4.26794919;11.39230485;0];
P4=[-7.73205081;9.39230485;0];
P5=[-7.73205081;-9.39230485;0];
P6=[-4.26794919;-11.39230485;0];

x1=x(1);
x2=x(2);
x3=x(3);
a=x(4);
b=x(5);
g=x(6);
l1=Length(1);
l2=Length(2);
l3=Length(3);
l4=Length(4);
l5=Length(5);
l6=Length(6);
X=[x1;x2;x3];
RX=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];
RY=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];
RZ=[cos(g) -sin(g) 0;sin(g) cos(g) 0; 0 0 1];
R=RZ*RY*RX;
f1=(norm(X+R*P1-B1))^2-l1^2;
f2=(norm(X+R*P2-B2))^2-l2^2;
f3=(norm(X+R*P3-B3))^2-l3^2;
f4=(norm(X+R*P4-B4))^2-l4^2;
f5=(norm(X+R*P5-B5))^2-l5^2;
f6=(norm(X+R*P6-B6))^2-l6^2;

df_f1_dx1 = diff(f1, x1); % df1/dx1
df_f1_dx2 = diff(f1, x2); % df1/dx2
df_f1_dx3 = diff(f1, x3); % df1/dx3
df_f1_dx4 = diff(f1, x4); % df1/dx4
df_f1_dx5 = diff(f1, x5); % df1/dx5
df_f1_dx6 = diff(f1, x6); % df1/dx6

df_f2_dx1 = diff(f2, x1); % df2/dx1
df_f2_dx2 = diff(f2, x2); % df2/dx2
df_f2_dx3 = diff(f2, x3); % df1/dx3
df_f2_dx4 = diff(f2, x4); % df1/dx4
df_f2_dx5 = diff(f2, x5); % df1/dx5
df_ff2_dx6 = diff(f2, x6); % df1/dx6

df_f3_dx1 = diff(f3, x1); % df3/dx1
df_f3_dx2 = diff(f3, x2); % df2/dx2
df_f3_dx3 = diff(f3, x3); % df1/dx3
df_f3_dx4 = diff(f3, x4); % df1/dx4
df_f3_dx5 = diff(f3, x5); % df1/dx5
df_f3_dx6 = diff(f3, x6); % df1/dx6

df_f4_dx1 = diff(f4, x1); % df4/dx1
df_f4_dx2 = diff(f4, x2); % df2/dx2
df_f4_dx3 = diff(f4, x3); % df1/dx3
df_f4_dx4 = diff(f4, x4); % df1/dx4
df_f4_dx5 = diff(f4, x5); % df1/dx5
df_f4_dx6 = diff(f4, x6); % df1/dx6

df_f5_dx1 = diff(f5, x1); % df5/dx1
df_f5_dx2 = diff(f5, x2); % df2/dx2
df_f5_dx3 = diff(f5, x3); % df1/dx3
df_f5_dx4 = diff(f5, x4); % df1/dx4
df_f5_dx5 = diff(f5, x5); % df1/dx5
df_f5_dx6 = diff(f5, x6); % df1/dx6

df_f6_dx1 = diff(f6, x1); % df6/dx1
df_f6_dx2 = diff(f6, x2); % df2/dx2
df_f6_dx3 = diff(f6, x3); % df1/dx3
df_f6_dx4 = diff(f6, x4); % df1/dx4
df_f6_dx5 = diff(f6, x5); % df1/dx5
df_f6_dx6 = diff(f6, x6); % df1/dx6

J= [df_f1_dx1,df_f1_dx2,df_f1_dx3,df_f1_dx4,df_f1_dx5,df_f1_dx6;
    df_f2_dx1,df_f2_dx2,df_f2_dx3,df_f2_dx4,df_f2_dx5,df_f2_dx6;
    df_f3_dx1,df_f3_dx2,df_f3_dx3,df_f3_dx4,df_f3_dx5,df_f3_dx6;
    df_f4_dx1,df_f4_dx2,df_f4_dx3,df_f4_dx4,df_f4_dx5,df_f4_dx6;
    df_f5_dx1,df_f5_dx2,df_f5_dx3,df_f5_dx4,df_f5_dx5,df_f5_dx6;
    df_f6_dx1,df_f6_dx2,df_f6_dx3,df_f6_dx4,df_f6_dx5,df_f6_dx6;   
]






% 
% J=[ 2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0),                      2.0*abs(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0), -2.0*abs(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*sign(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a)),   2.0*abs(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*(2.0*cos(g)*sin(a) - 2.0*cos(a)*sin(b)*sin(g)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*sin(a)*sin(g) + 2.0*cos(a)*cos(g)*sin(b)) + 4.0*abs(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*sign(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*cos(a)*cos(b),                                                2.0*abs(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*sign(12.0*sin(b) - 1.0*x3 + 2.0*cos(b)*sin(a))*(12.0*cos(b) - 2.0*sin(a)*sin(b)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(12.0*cos(g)*sin(b) + 2.0*cos(b)*cos(g)*sin(a)) - 2.0*abs(x2 - 2cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*(12.0*sin(b)*sin(g) + 2.0*cos(b)*sin(a)*sin(g)),                                                   2.0*abs(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*sign(x2 - 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) - 2.0*sin(a)*sin(b)*sin(g) + 14.0)*(12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b)) + 2.0*abs(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) + 2.0*cos(a)*sin(g) - 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*cos(a)*cos(g) - 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g));
%     2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0),                      2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0),          2.0*abs(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*sign(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a)),   2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*sin(a)*sin(g) + 2.0*cos(a)*cos(g)*sin(b)) - 2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*(2.0*cos(g)*sin(a) - 2.0*cos(a)*sin(b)*sin(g)) + 4.0*abs(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*sign(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*cos(a)*cos(b),                                                      - 2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*(12.0*sin(b)*sin(g) - 2.0*cos(b)*sin(a)*sin(g)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(12.0*cos(g)*sin(b) - 2.0*cos(b)*cos(g)*sin(a)) - 2.0*abs(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*sign(x3 - 12.0*sin(b) + 2.0*cos(b)*sin(a))*(12.0*cos(b) + 2.0*sin(a)*sin(b)),                                                   2.0*abs(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*sign(x2 + 2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g) - 14.0)*(12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b)) - 2.0*abs(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*sign(x1 + 12.0*cos(b)*cos(g) - 2.0*cos(a)*sin(g) + 2.0*cos(g)*sin(a)*sin(b) - 12.0)*(2.0*cos(a)*cos(g) + 12.0*cos(b)*sin(g) + 2.0*sin(a)*sin(b)*sin(g));
%     -2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124),          2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39),    2.0*abs(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a)),  22.78*abs(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*cos(a)*cos(b) - 2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(11.39*cos(g)*sin(a) - 11.39*cos(a)*sin(b)*sin(g)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*sin(a)*sin(g) + 11.39*cos(a)*cos(g)*sin(b)),         2.0*abs(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) + 11.39*cos(b)*sin(a))*(4.268*cos(b) - 11.39*sin(a)*sin(b)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(4.268*cos(g)*sin(b) + 11.39*cos(b)*cos(g)*sin(a)) + 2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*sin(b)*sin(g) + 11.39*cos(b)*sin(a)*sin(g)),         2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g)) - 2.0*abs(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(x2 + 11.39*cos(a)*cos(g) - 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*cos(b)*cos(g) + 11.39*cos(a)*sin(g) - 11.39*cos(g)*sin(a)*sin(b));
%     2.0*abs(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12),          2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392),    2.0*abs(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a)), 2.0*abs(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*sin(a)*sin(g) + 9.392*cos(a)*cos(g)*sin(b)) - 2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(9.392*cos(g)*sin(a) - 9.392*cos(a)*sin(b)*sin(g)) + 18.78*abs(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*cos(a)*cos(b),                 2.0*abs(x1 - 7.732*cos(b)*cos(g) -in(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(7.732*cos(g)*sin(b) + 9.392*cos(b)*cos(g)*sin(a)) + 2.0*abs(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) + 9.392*cos(b)*sin(a))*(7.732*cos(b) - 9.392*sin(a)*sin(b)) + 2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*sin(b)*sin(g) + 9.392*cos(b)*sin(a)*sin(g)),               - 2.0*abs(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g)) - 2.0*abs(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(x2 + 9.392*cos(a)*cos(g) - 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b));
%     2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12), -2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392),    2.0*abs(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a)), - 2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(9.392*cos(g)*sin(a) - 9.392*cos(a)*sin(b)*sin(g)) - 2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*sin(a)*sin(g) + 9.392*cos(a)*cos(g)*sin(b)) - 18.78*abs(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*cos(a)*cos(b),         2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(7.732*cos(g)*sin(b) - 9.392*cos(b)*cos(g)*sin(a)) + 2.0*abs(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*sign(x3 + 7.732*sin(b) - 9.392*cos(b)*sin(a))*(7.732*cos(b) + 9.392*sin(a)*sin(b)) - 2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*sin(b)*sin(g) - 9.392*cos(b)*sin(a)*sin(g)),         2.0*abs(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*sign(x1 - 7.732*cos(b)*cos(g) + 9.392*cos(a)*sin(g) - 9.392*cos(g)*sin(a)*sin(b) + 18.12)*(9.392*cos(a)*cos(g) + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g)) + 2.0*abs(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*sign(9.392*cos(a)*cos(g) - 1.0*x2 + 7.732*cos(b)*sin(g) + 9.392*sin(a)*sin(b)*sin(g) - 3.392)*(7.732*cos(b)*cos(g) - 9.392*cos(a)*sin(g) + 9.392*cos(g)*sin(a)*sin(b));
%     -2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124), -2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39),    2.0*abs(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a)), 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*sin(a)*sin(g) + 11.39*cos(a)*cos(g)*sin(b)) - 2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(11.39*cos(g)*sin(a) - 11.39*cos(a)*sin(b)*sin(g)) - 22.78*abs(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*cos(a)*cos(b), 2.0*abs(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*sign(x3 + 4.268*sin(b) - 11.39*cos(b)*sin(a))*(4.268*cos(b) + 11.39*sin(a)*sin(b)) - 2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*sin(b)*sin(g) - 11.39*cos(b)*sin(a)*sin(g)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(4.268*cos(g)*sin(b) - 11.39*cos(b)*cos(g)*sin(a)), 2.0*abs(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*sign(11.39*cos(a)*cos(g) - 1.0*x2 + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g) - 17.39)*(4.268*cos(b)*cos(g) - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b)) - 2.0*sign(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*abs(4.268*cos(b)*cos(g) - 1.0*x1 - 11.39*cos(a)*sin(g) + 11.39*cos(g)*sin(a)*sin(b) + 6.124)*(11.39*cos(a)*cos(g) + 4.268*cos(b)*sin(g) + 11.39*sin(a)*sin(b)*sin(g));
%  ];
end


